#ifndef PATH_CONTROLLER_H
#define PATH_CONTROLLER_H

#include "JointTrackingController.h"
#include "Modeling/DynamicPath.h"
#include <list>

/** @brief A controller that allows multiple milestone commands to be
 * queued up in order to execute a long trajectory.  Uses a DynamicPath
 * structure to store the queue.
 *
 * The segment that is currently being executed (segment 0) is stored in the
 * 'ramp' member of the MilestoneTrackingController superclass.  Segments
 * 1-n are stored in the 'path' member.
 */
class MilestonePathController : public MilestoneTrackingController
{
 public:
  MilestonePathController(Robot& robot);
  void SetSpeedLimits(Real velScale,Real accScale);  //Note: does not affect current path

  void SetPath(const vector<Vector>& path);
  void SetPath(const vector<Vector>& path,const vector<Vector>& pathDeriv);
  void SetPath(const DynamicPath& path);
  void AddMilestone(const Vector& x);
  void AddMilestone(const Vector& x,const Vector& dx);
  Real AddMilestone(const Vector& x,const Vector& dx,Real dt);
  void GetPath(DynamicPath& path) const;
  const Config& Endpoint() const;
  const Vector& EndpointVelocity() const;
  bool Done() const { return path.Empty() && MilestoneTrackingController::Reached(); }
  Real TimeRemaining() const { return path.GetTotalTime() + MilestoneTrackingController::TimeRemaining(); }

  virtual void Update(Real dt);  
  virtual void Reset();
  virtual bool ReadState(File& f);
  virtual bool WriteState(File& f) const;

  DynamicPath path;
};

#endif
